#include <gtest/gtest.h>
#include <gmock/gmock.h>
#include "axisgroup/axisgroup.h"
#include "axisgroup/axisgroup_robot.h"
#include "axis/axis.h"
#include "kine/kine.h"
#include <math.h>

#include <thread>

#define TEST_COUNT 10

using namespace axisgroup;

class MockAxis : public axis::Axis
{
public:
    MOCK_METHOD0(run_state, RunState());
    MOCK_METHOD0(get_id, int());
};

class MockRobot : public Robot
{
public:
    MOCK_METHOD0(update_fdb_state, void());
    MOCK_METHOD0(update_plan_mode, void());
    MOCK_METHOD0(update_pose_offset, void());
    MOCK_METHOD0(update_fsm_mode, void());
    MOCK_METHOD0(is_booting, bool());

    MOCK_METHOD0(get_state, AxisGroup::FsmState());
};

class RobotTest : public ::testing::Test
{
protected:
    virtual void SetUp() override
    {
        // TestRobot test;
        // test.update();
        // test.get_state();
        // test.AxisGroup::get_state();
        for (int i = 0; i < 7; i++) { EXPECT_CALL(joint[i], get_id()).WillRepeatedly(testing::Return(i)); }
        robot = std::make_shared<MockRobot>();
        robot->reset();
        ASSERT_EQ(robot->link_joint(&joint[0]), 0);
        ASSERT_EQ(robot->link_joint(&joint[1]), 0);
        ASSERT_EQ(robot->link_joint(&joint[2]), 0);
        ASSERT_EQ(robot->link_joint(&joint[3]), 0);
        ASSERT_EQ(robot->link_joint(&joint[4]), 0);
        ASSERT_EQ(robot->link_joint(&joint[5]), 0);
        ASSERT_EQ(robot->joint_num(), 6);

        EXPECT_CALL(joint[0], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
        EXPECT_CALL(joint[1], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
        EXPECT_CALL(joint[2], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
        EXPECT_CALL(joint[3], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
        EXPECT_CALL(joint[4], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
        EXPECT_CALL(joint[5], run_state()).WillRepeatedly(testing::Return(axis::Axis::RunState::Standstill));
    }
    virtual void TearDown() override {}

    MockAxis joint[7];
    std::shared_ptr<MockRobot> robot;
};

TEST_F(RobotTest, Init)
{
    robot->update();

    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);

    robot->update();
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_FREE);
}

TEST_F(RobotTest, CANTSWITCH)
{
    robot->do_enable(1);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);
    EXPECT_CALL(*robot, get_state()).WillRepeatedly(testing::Return(AxisGroup::FsmState::Disabled));  // booting 完成
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);
    EXPECT_CALL(*robot, get_state()).WillRepeatedly(testing::Return(AxisGroup::FsmState::ErrorStop));  // booting 完成
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);
    EXPECT_CALL(*robot, get_state()).WillRepeatedly(testing::Return(AxisGroup::FsmState::Moving));  // booting 完成
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);
    EXPECT_CALL(*robot, get_state()).WillRepeatedly(testing::Return(AxisGroup::FsmState::Stopping));  // booting 完成
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_IDLE);
}

TEST_F(RobotTest, SWITCH)
{
    EXPECT_CALL(*robot, get_state()).WillRepeatedly(testing::Return(AxisGroup::FsmState::Standby));  // booting 完成
    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_FREE);
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_FREE);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_FREE);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_DRAG);
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_DRAG);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_DRAG);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_SERVO);
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_SERVO);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_SERVO);

    robot->switch_plan_mode(PFsmState::PlanMode::PLAN_COORD);
    robot->update();
    EXPECT_EQ(robot->get_target_plan_mode(), PFsmState::PlanMode::PLAN_COORD);
    EXPECT_EQ(robot->get_cur_plan_mode(), PFsmState::PlanMode::PLAN_COORD);
}